#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "agv_srvs/srv/set_laser_calib.hpp"
#include <shared_parameters/shared_params_interface.hpp>
#include <memory>

using std::placeholders::_1;
using std::placeholders::_2;

struct DynamicParam {
    bool enable = false;
    bool isInversion = false;
    double pose_x = 0;
    double pose_y = 0;
    double pose_z = 0;
    double pose_theta = 0;

    bool back_enable = false;
    bool back_isInversion = false;
    double back_pose_x = 0;
    double back_pose_y = 0;
    double back_pose_z = 0;
    double back_pose_theta = 0;
    double freq = 100.0;
} dynamic_;

class LaserTransformPublisher : public rclcpp::Node {
public:
    LaserTransformPublisher() : Node("laser_transform_publisher") {
        // 初始化参数
        init_parameters();

        // 获取参数值
        dynamic_.pose_x = this->get_parameter_or("lidar1/laser_pose_x", 0.0);
        dynamic_.pose_y = this->get_parameter_or("lidar1/laser_pose_y", 0.0);
        dynamic_.pose_theta = this->get_parameter_or("lidar1/laser_pose_theta", 0.0);
        dynamic_.isInversion = this->get_parameter_or("lidar1/isInversion", false);
        dynamic_.enable = this->get_parameter_or("lidar1/enable", true);

        dynamic_.back_pose_x = this->get_parameter_or("lidar2/laser_pose_x", 0.0);
        dynamic_.back_pose_y = this->get_parameter_or("lidar2/laser_pose_y", 0.0);
        dynamic_.back_pose_theta = this->get_parameter_or("lidar2/laser_pose_theta", 0.0);
        dynamic_.back_isInversion = this->get_parameter_or("lidar2/isInversion", false);
        dynamic_.back_enable = this->get_parameter_or("lidar2/enable", true);
        dynamic_.freq = this->get_parameter_or("frequency", 100.0);

        RCLCPP_INFO(this->get_logger(), "============= 激光雷达参数配置 =============");
        RCLCPP_INFO(this->get_logger(), "前向雷达:");
        RCLCPP_INFO(this->get_logger(), "  X: %.3f m", dynamic_.pose_x);
        RCLCPP_INFO(this->get_logger(), "  Y: %.3f m", dynamic_.pose_y);
        RCLCPP_INFO(this->get_logger(), "  θ: %.3f rad", dynamic_.pose_theta);
        RCLCPP_INFO(this->get_logger(), "  倒置: %s", dynamic_.isInversion ? "是" : "否");
        RCLCPP_INFO(this->get_logger(), "  启用: %s", dynamic_.enable ? "是" : "否");

        RCLCPP_INFO(this->get_logger(), "后向雷达:");
        RCLCPP_INFO(this->get_logger(), "  X: %.3f m", dynamic_.back_pose_x);
        RCLCPP_INFO(this->get_logger(), "  Y: %.3f m", dynamic_.back_pose_y);
        RCLCPP_INFO(this->get_logger(), "  θ: %.3f rad", dynamic_.back_pose_theta);
        RCLCPP_INFO(this->get_logger(), "  倒置: %s", dynamic_.back_isInversion ? "是" : "否");
        RCLCPP_INFO(this->get_logger(), "  启用: %s", dynamic_.back_enable ? "是" : "否");

        RCLCPP_INFO(this->get_logger(), "发布频率: %.1f Hz", dynamic_.freq);
        RCLCPP_INFO(this->get_logger(), "==========================================");

        // 初始化TF广播器（使用动态广播器）
        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);

        // 创建服务
        laser_calib_service_ = this->create_service<agv_srvs::srv::SetLaserCalib>(
            "/setLaserCalib",
            std::bind(&LaserTransformPublisher::HandleSetLaser, this, _1, _2));

        // 创建定时器
        timer_ = this->create_wall_timer(
            std::chrono::duration<double>(1.0 / dynamic_.freq),
            std::bind(&LaserTransformPublisher::timer_callback, this));
    }

private:
    void timer_callback() {
        auto now = this->now();

        // 发布第一个激光雷达的TF
        geometry_msgs::msg::TransformStamped transformStamped;
        transformStamped.header.stamp = now;
        transformStamped.header.frame_id = "base_link";
        transformStamped.child_frame_id = "laser_link_1";
        transformStamped.transform.translation.x = dynamic_.pose_x;
        transformStamped.transform.translation.y = dynamic_.pose_y;
        transformStamped.transform.translation.z = dynamic_.pose_z;
        
        tf2::Quaternion quat;
        if (dynamic_.isInversion) {
            quat.setRPY(M_PI, 0.0, dynamic_.pose_theta);
        } else {
            quat.setRPY(0.0, 0.0, dynamic_.pose_theta);
        }

        transformStamped.transform.rotation.x = quat.x();
        transformStamped.transform.rotation.y = quat.y();
        transformStamped.transform.rotation.z = quat.z();
        transformStamped.transform.rotation.w = quat.w();

        if (dynamic_.enable) {
            tf_broadcaster_->sendTransform(transformStamped);
        }

        // 发布第二个激光雷达的TF
        geometry_msgs::msg::TransformStamped back_transformStamped;
        back_transformStamped.header.stamp = now;
        back_transformStamped.header.frame_id = "base_link";
        back_transformStamped.child_frame_id = "laser_link_2";
        back_transformStamped.transform.translation.x = dynamic_.back_pose_x;
        back_transformStamped.transform.translation.y = dynamic_.back_pose_y;
        back_transformStamped.transform.translation.z = dynamic_.back_pose_z;
        
        tf2::Quaternion back_quat;
        if (dynamic_.back_isInversion) {
            back_quat.setRPY(M_PI, 0.0, dynamic_.back_pose_theta);
        } else {
            back_quat.setRPY(0.0, 0.0, dynamic_.back_pose_theta);
        }

        back_transformStamped.transform.rotation.x = back_quat.x();
        back_transformStamped.transform.rotation.y = back_quat.y();
        back_transformStamped.transform.rotation.z = back_quat.z();
        back_transformStamped.transform.rotation.w = back_quat.w();

        if (dynamic_.back_enable) {
            tf_broadcaster_->sendTransform(back_transformStamped);
        }
    }

    bool HandleSetLaser(
        const std::shared_ptr<agv_srvs::srv::SetLaserCalib::Request> request,
        std::shared_ptr<agv_srvs::srv::SetLaserCalib::Response> response) {

        if (request->laser_num == 1) {
            dynamic_.pose_x = request->x[0];
            dynamic_.pose_y = request->y[0];
            dynamic_.pose_theta = request->yaw[0];
            dynamic_.isInversion = request->isinversion[0];
            dynamic_.enable = request->enable[0];

            dynamic_.back_enable = false;
            RCLCPP_INFO(this->get_logger(), "set single lidar is OK!");
            response->error = "";
            response->success = true;
        } else if (request->laser_num == 2) {
            dynamic_.pose_x = request->x[0];
            dynamic_.pose_y = request->y[0];
            dynamic_.pose_theta = request->yaw[0];
            dynamic_.isInversion = request->isinversion[0];
            dynamic_.enable = request->enable[0];

            dynamic_.back_pose_x = request->x[1];
            dynamic_.back_pose_y = request->y[1];
            dynamic_.back_pose_theta = request->yaw[1];
            dynamic_.back_isInversion = request->isinversion[1];
            dynamic_.back_enable = request->enable[1];
            RCLCPP_INFO(this->get_logger(), "set two lidars is OK!");
            response->error = "";
            response->success = true;
        } else {
            response->error = "the num of lidar is not between 1 and 2!";
            response->success = false;
        }

        return true;
    }

    bool init_parameters() {
        // 获取共享内存参数
        if (!SharedParamsInterface::initialize()) {
            RCLCPP_ERROR(get_logger(), "Failed to initialize shared memory interface");
            return false;
        }

        try { 
            auto params = SharedParamsInterface::getNodeParams("shared_parameters");
            if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
                RCLCPP_INFO(get_logger(), "Error getting shared parameters");
                return false;
            } 

            params = SharedParamsInterface::getNodeParams("lidar_guidance");
            for (const auto& [name, value] : params) {
                if (name.find("lidar1") == std::string::npos && 
                    name.find("lidar2") == std::string::npos)
                    continue;
                
                std::string param_name;
                const std::string prefix = "lidar_info/";
                size_t pos = name.find(prefix);

                if (pos != std::string::npos) {
                    size_t start_pos = pos + prefix.length();
                    
                    if (start_pos <= name.length()) {
                        param_name = name.substr(start_pos); 
                    } else {
                        continue;
                    }
                } else {
                    continue;
                }
                
                RCLCPP_INFO_STREAM(get_logger(), "get param:" << param_name << ", value:" << value.toString());
                switch(value.type) {
                case SharedParamsInterface::ParamValue::Type::INT:
                    this->declare_parameter(param_name, value.getValue<int>());
                    break;
                case SharedParamsInterface::ParamValue::Type::DOUBLE:
                    this->declare_parameter(param_name, value.getValue<double>());
                    break;
                case SharedParamsInterface::ParamValue::Type::STRING:
                    this->declare_parameter(param_name, value.getValue<std::string>());
                    break;
                case SharedParamsInterface::ParamValue::Type::BOOL:
                    this->declare_parameter(param_name, value.getValue<bool>());
                    break;
                default:
                    RCLCPP_WARN_STREAM(get_logger(), "Unsupported parameter type for " << name);
                    break;
                }
            }
        } catch (const std::exception& e) {
            RCLCPP_ERROR_STREAM(get_logger(), "Error reading parameters: " << e.what());
            return false;
        }
        return true;
    }

    
    rclcpp::Service<agv_srvs::srv::SetLaserCalib>::SharedPtr laser_calib_service_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<LaserTransformPublisher>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}